﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework;

namespace MatlabIG
{
    class Hud
    {
        SpriteFont FontAlpha;
        SpriteBatch spriteBatch;
        MatlabModel mainModel;
        VertexBuffer vertexbuffer;
        BasicEffect basicEffect;
        List<VertexPositionColor> verts;

        struct STextDrawChunk
        {
            public string text;
            public Vector2 destination;

            public STextDrawChunk(string str, Vector2 vec)
            {
                text = str;
                destination = vec;
            }
        };

        List<STextDrawChunk> textDrawList = new List<STextDrawChunk>();

        public void Init(MatlabModel mMod)
        {
            FontAlpha = Globals.Content.Load<SpriteFont>("Courier New");
            spriteBatch = new SpriteBatch(Globals.Graphics.GraphicsDevice);
            mainModel = mMod;

            // init vertex buffer + basic effect
            vertexbuffer = new VertexBuffer(Globals.Graphics.GraphicsDevice, typeof(VertexPositionColor), 10000, BufferUsage.None);
            //vertexbuffer.SetData(vertices);
            //totalLinesCount = vertices.Length / 2
            basicEffect = new BasicEffect(Globals.Graphics.GraphicsDevice);
        }

        public void Draw()
        {
	        // crtaj linije
            textDrawList.Clear();
            DrawLines();
            
            spriteBatch.Begin();
            // Draw model position
            string output = mainModel.GetPositionString();
            // Find the center of the string
            Vector2 fontPos = new Vector2(10, 10);
            //spriteBatch.DrawString(FontAlpha, output, fontPos, Color.Yellow, 0, Vector2.Zero, 1.0f, SpriteEffects.None, 0.5f);
            
            // draw all strings in the list
            foreach (STextDrawChunk chunk in textDrawList)
            {
                Vector2 dest;
                dest.X = (chunk.destination.X / (1000.0f * Globals.Graphics.GraphicsDevice.Viewport.AspectRatio)) * Globals.Graphics.GraphicsDevice.Viewport.Width + Globals.Graphics.GraphicsDevice.Viewport.Width / 2.0f;
                dest.Y = chunk.destination.Y / (1000.0f) * Globals.Graphics.GraphicsDevice.Viewport.Height + Globals.Graphics.GraphicsDevice.Viewport.Height / 2.0f; 
                spriteBatch.DrawString(FontAlpha, chunk.text, dest, Color.White, 0, new Vector2(10, 10), 1.0f, SpriteEffects.None, 0.5f);
            }
            spriteBatch.End();
        }

        private void DrawLines()
        {
            // create lines
            verts = new List<VertexPositionColor>();

            double pitch = -MathHelper.ToDegrees((float)mainModel.RPY[1]);
            double rollRad = -mainModel.RPY[0];

            // draw mark in the middle
            Color c = Color.Green;
	        Vector2[] pts = new Vector2[6];
	        pts[0] = new Vector2(-50, 0);	
	        pts[1] = new Vector2(-20, 0);	
	        pts[2] = new Vector2(0, -20);	
	        pts[3] = new Vector2(20, 0);
            pts[4] = new Vector2(50, 0);
            AddLine(pts[0], pts[1], c);
            AddLine(pts[1], pts[2], c);
            AddLine(pts[2], pts[3], c);
            AddLine(pts[3], pts[4], c);
            //AddLine(pts[4], pts[5], c);

            AddLine(-70, -100, -30, -100, c);
            AddLine(70, -100, 30, -100, c);
            AddLine(0, -40, 0, -70, c);

            // draw artifical horizon
	        // rotation
            Vector2 midpoint = new Vector2(0, 0);
	        Vector2 rotated = new Vector2( (float)Math.Cos(-rollRad), (float)Math.Sin(-rollRad) );
	        Vector2 transformed0  = new Vector2(rotated.X*200.0f, rotated.Y*200.0f);
	        Vector2 transformed1 = new Vector2 (rotated.X*600.0f, rotated.Y*600.0f);

            AddLine( transformed0.X + midpoint.X, transformed0.Y + midpoint.Y, transformed1.X + midpoint.X, transformed1.Y + midpoint.Y, Color.White);
            AddLine(-transformed0.X + midpoint.X, -transformed0.Y + midpoint.Y, -transformed1.X + midpoint.X, -transformed1.Y + midpoint.Y, Color.White);
            
	        for(float i=-90; i<=90; i+=10)
	        {
		        if( i == 0 ) continue;
		        Vector2 offset = new Vector2((float)-Math.Sin(-rollRad), (float)Math.Cos(-rollRad));
		        Vector2 translation1 = new Vector2(offset.X*(1000/45.0f*((float)pitch+i)), offset.Y*(1000/45.0f*((float)pitch+i)));
		        transformed0.X = rotated.X*300 + translation1.X;
		        transformed0.Y = rotated.Y*300 + translation1.Y;
		        transformed1.X = rotated.X*400 + translation1.X;
		        transformed1.Y = rotated.Y*400 + translation1.Y;
                AddLine(transformed0.X + midpoint.X, transformed0.Y + midpoint.Y, transformed1.X + midpoint.X, transformed1.Y + midpoint.Y, Color.White);
		
		        transformed0.X = -rotated.X*300 + translation1.X;
		        transformed0.Y = -rotated.Y*300 + translation1.Y;
		        transformed1.X = -rotated.X*400 + translation1.X;
		        transformed1.Y = -rotated.Y*400 + translation1.Y;
                AddLine(transformed0.X + midpoint.X, transformed0.Y + midpoint.Y, transformed1.X + midpoint.X, transformed1.Y + midpoint.Y, Color.White);

                string str;
                str = string.Format("{0}", i.ToString());
                Vector2 translation2 = new Vector2(offset.X * (1000 / 45.0f * ((float)pitch + i)), offset.Y * (1000 / 45.0f * ((float)pitch + i)));
		        transformed0.X = rotated.X*240 + translation2.X;
		        transformed0.Y = -(rotated.Y*240 + translation2.Y);
                textDrawList.Add(new STextDrawChunk(str, transformed0 + midpoint));

                Vector2 translation3 = new Vector2(offset.X * (1000 / 45.0f * ((float)pitch + i)), offset.Y * (1000 / 45.0f * ((float)pitch + i)));
		        transformed0.X = -rotated.X*260 + translation3.X;
		        transformed0.Y = -(-rotated.Y*260 + translation3.Y);
                textDrawList.Add(new STextDrawChunk(str, transformed0 + midpoint));
	        }
	        for(float i=-85; i<=90; i+=10)
	        {
		        Vector2 offset = new Vector2((float)-Math.Sin(-rollRad), (float)Math.Cos(-rollRad));
		        Vector2 translation1 = new Vector2(offset.X*(1000/45*((float)pitch+i)), offset.Y*(1000/45*((float)pitch+i)));
		        transformed0.X = rotated.X*330 + translation1.X;
		        transformed0.Y = rotated.Y*330 + translation1.Y;
		        transformed1.X = rotated.X*370 + translation1.X;
		        transformed1.Y = rotated.Y*370 + translation1.Y;
                AddLine(transformed0.X + midpoint.X, transformed0.Y + midpoint.Y, transformed1.X + midpoint.X, transformed1.Y + midpoint.Y, Color.White);
		
		        transformed0.X = -rotated.X*330 + translation1.X;
		        transformed0.Y = -rotated.Y*330 + translation1.Y;
		        transformed1.X = -rotated.X*370 + translation1.X;
		        transformed1.Y = -rotated.Y*370 + translation1.Y;
                AddLine(transformed0.X + midpoint.X, transformed0.Y + midpoint.Y, transformed1.X + midpoint.X, transformed1.Y + midpoint.Y, Color.White);
	        }
	
	        // zero line
	        {
		        Vector2 offset = new Vector2((float)-Math.Sin(-rollRad), (float)Math.Cos(-rollRad));
		        Vector2 translation1 = new Vector2(offset.X*(1000/45*((float)pitch+0)), offset.Y*(1000/45*((float)pitch+0)));
		        transformed0.X = rotated.X*200 + translation1.X;
		        transformed0.Y = rotated.Y*200 + translation1.Y;
		        transformed1.X = rotated.X*500 + translation1.X;
		        transformed1.Y = rotated.Y*500 + translation1.Y;
                AddLine(transformed0.X + midpoint.X, transformed0.Y + midpoint.Y, transformed1.X + midpoint.X, transformed1.Y + midpoint.Y, Color.GreenYellow);
		
		        transformed0.X = -rotated.X*200 + translation1.X;
		        transformed0.Y = -rotated.Y*200 + translation1.Y;
		        transformed1.X = -rotated.X*500 + translation1.X;
		        transformed1.Y = -rotated.Y*500 + translation1.Y;
                AddLine(transformed0.X + midpoint.X, transformed0.Y + midpoint.Y, transformed1.X + midpoint.X, transformed1.Y + midpoint.Y, Color.GreenYellow);
	        }

            // altitude line
	        double off = mainModel.Z*10;
            AddLine(700, (float)off + midpoint.Y, 700, 100000.0f, Color.Green);
	        for(int i=0; i!=10000; i+=10)
	        {
                AddLine(700, (float)off + midpoint.Y + i * 10, 720, (float)off + midpoint.Y + i * 10, Color.Green);

                string str = string.Format("{0}", i.ToString());
                Vector2 point = new Vector2(730, -(float)off - i * 10.0f - 15.0f);
                textDrawList.Add(new STextDrawChunk(str, point));
	        }
            
	        Vector2 offs3 = new Vector2(700, 0);
            Vector2[] points = new Vector2[6];
	        points[0] = new Vector2(-200, -20) + offs3;	
	        points[1]  = new Vector2(-40, -20) + offs3;	
	        points[2]  = new Vector2(0, 0) + offs3;	
	        points[3]  = new Vector2(-40, 20) + offs3;	
	        points[4]  = new Vector2(-200, 20) + offs3;	
	        points[5]  = new Vector2(-200, -20) + offs3;
            AddLine(points[0], points[1], Color.Green);
            AddLine(points[1], points[2], Color.Green);
            AddLine(points[2], points[3], Color.Green);
            AddLine(points[3], points[4], Color.Green);
            AddLine(points[4], points[5], Color.Green);
            Vector2 offs4 = new Vector2(550.0f, 0.0f);
            string st2 = string.Format("{0:0.000}", -mainModel.Z);
            textDrawList.Add(new STextDrawChunk(st2, offs4));

            // speed line
            double speed = Math.Sqrt(Math.Pow(mainModel.Velocities[0], 2) + Math.Pow(mainModel.Velocities[1], 2));
            off = -speed * 100;
            AddLine(-700, (float)off, -700, 100000.0f, Color.Green);
	        for(int i=0; i!=100; i+=1)
	        {
                AddLine(-700, (float)off + i * 100, -720, (float)off + i * 100, Color.Green);

                string str = string.Format("{0}", i.ToString());
                Vector2 point = new Vector2(-730, -(float)off - i * 100.0f - 15.0f);
                textDrawList.Add(new STextDrawChunk(str, point));
	        }

            offs3 = new Vector2(-700, 0);
            points = new Vector2[6];
            points[0] = new Vector2(200, -20) + offs3;
            points[1] = new Vector2(40, -20) + offs3;
            points[2] = new Vector2(0, 0) + offs3;
            points[3] = new Vector2(40, 20) + offs3;
            points[4] = new Vector2(200, 20) + offs3;
            points[5] = new Vector2(200, -20) + offs3;
            AddLine(points[0], points[1], Color.Green);
            AddLine(points[1], points[2], Color.Green);
            AddLine(points[2], points[3], Color.Green);
            AddLine(points[3], points[4], Color.Green);
            AddLine(points[4], points[5], Color.Green);
            offs4 = new Vector2(-620.0f, 0.0f);
            st2 = string.Format("{0:0.0}", speed);
            textDrawList.Add(new STextDrawChunk(st2, offs4));

            // yaw
            float yaw = MathHelper.ToDegrees((float)mainModel.RPY[2]);
	        int ii=0;
	        for(double i=-40-(yaw-(int)(yaw/5)*5); i<=40; i+=5)
	        {
		        ii++;
		        Vector2 mid = new Vector2(0, 700);
		        Vector2 pt1 = new Vector2((float)Math.Sin(i*Math.PI/180)*300, -(float)Math.Cos(i*Math.PI/180)*300);
		        if( ((ii*5+yaw)/10-(int)((ii*5+yaw)/10)) > 0.5 )
		        {
                    Vector2 pt2 = new Vector2((float)Math.Sin(i * Math.PI / 180) * 340, -(float)Math.Cos(i * Math.PI / 180) * 340);
                    AddLine(pt1 + mid, pt2 + mid, Color.Gold);
		        }
		        else
		        {
			        Vector2 pt2 = new Vector2((float)Math.Sin(i*Math.PI/180)*320, -(float)Math.Cos(i*Math.PI/180)*320);
                    AddLine(pt1 + mid, pt2 + mid, Color.Gold);
		        }
	        }
            
	        Vector2 offs7 = new Vector2(0, 410);
	        points[0] = new Vector2(-20, 40) + offs7;	
	        points[1] = new Vector2(0, 0) + offs7;	
	        points[2] = new Vector2(0, 0) + offs7;	
	        points[3] = new Vector2(20, 40) + offs7;	
	        points[4] = new Vector2(-20, 40) + offs7;
            AddLine(points[0], points[1], Color.Gold);
            AddLine(points[1], points[2], Color.Gold);
            AddLine(points[2], points[3], Color.Gold);
            AddLine(points[3], points[4], Color.Gold);
            
            offs4 = new Vector2(-10, -350);
            st2 = string.Format("{0:0.0}", (int)yaw);
            textDrawList.Add(new STextDrawChunk(st2, offs4));



            // set to buffer and draw!
            VertexPositionColor[] vertices = verts.ToArray(); 
            vertexbuffer.SetData(vertices);

            // set effect parameters
            float aspectRatio = Globals.Graphics.GraphicsDevice.Viewport.AspectRatio;
            basicEffect.World = Matrix.Identity;
            basicEffect.View = Matrix.Identity;
            basicEffect.Projection = Matrix.CreateOrthographic(1000*aspectRatio, 1000, 0, 1);
            basicEffect.LightingEnabled = false;
            basicEffect.VertexColorEnabled = true;

            foreach (EffectPass pass in basicEffect.CurrentTechnique.Passes)
            {
                pass.Apply();
                Globals.Graphics.GraphicsDevice.SetVertexBuffer(vertexbuffer);
                Globals.Graphics.GraphicsDevice.DrawPrimitives(PrimitiveType.LineList, 0, vertices.Length/2);
            }
        }

        private void AddLine(Vector2 v1, Vector2 v2, Color color)
        {
            AddLine(v1.X, v1.Y, v2.X, v2.Y, color);
        }

        private void AddLine(float p1, float p2, float p3, float p4, Color color)
        {
            VertexPositionColor v1 = new VertexPositionColor(new Vector3(p1, p2, 0), color);
            verts.Add(v1);
            VertexPositionColor v2 = new VertexPositionColor(new Vector3(p3, p4, 0), color);
            verts.Add(v2);
        }
    }
}
